package com.arithmetic;

import android.os.Message;
import android.util.Log;

import com.DataProcess.Robot;
import com.pudu.mydemo.entity.Contant;
import com.pudu.mydemo.nativeClass.NativeClass;
import com.pudu.pudusclient.ServerAct;
import com.sensors.Lidar;
import com.tool.Distance;

/**
 * Created by tjpu on 2016/10/15.
 */
public class StatusCheck {
    static String thisLogTag = "StatusCheck";

    static int mapCnt = 0;
    public static void checkMap() {
        mapCnt ++;
        if (mapCnt < 50)
            return;
        else{
            mapCnt = 0;
            if (Robot.uwbStation == null || Robot.track == null) {
                Message msg = ServerAct.mHandler.obtainMessage();
                msg.what = 0x14;
                msg.obj = "没有地图，请发送地图";
                ServerAct.mHandler.sendMessage(msg);
                InitMap.initMap();
                InitNative.initNative();
                Log.e(thisLogTag,"no map");
            }
        }
    }

    int powerCnt = 0;
    public void checkPower() {
        powerCnt ++;
        if(powerCnt < 50)
            return;
        else{
            if(Robot.isLowPower)
                Log.i(thisLogTag,"low power");
        }
    }

    static double OBSTACLE_STOP_TIME = 1.5;
    static double OBSTACLE_SLOW_TIME = 3.0;

    public static double checkObstacle() {
        double time = Lidar.getCloseTime();
        double adjust;
        Log.i(thisLogTag,"Obstacle time: " + time);
        if (time < OBSTACLE_STOP_TIME ) {
//            Robot.isObstacle = true;
            adjust = 0.0;
        } else if (time > OBSTACLE_STOP_TIME  && time <OBSTACLE_SLOW_TIME)
            adjust = time / OBSTACLE_SLOW_TIME - 0.1;
        else
            adjust = 1.0;

        Log.i(thisLogTag,"Obstacle adjust: " + adjust);
        return adjust;
    }

    public void checkReceive() {

    }

    static double INSIDE_SOURCE = 0.5;
    static double OUTSIDE_SOURCE = 1.0;
    public static void checkSource() {
        if(!Robot.mode.equals(Contant.ACTION_THROUGH_MOVE))
            return;
        if (Robot.sourcePos == null || Robot.sourcePos.length == 0){
            Log.e(thisLogTag,"no sourcePos");
            return;
        }

        double distance = Distance.vector2D(Robot.position,Robot.sourcePos);

        if (Robot.isInsideSource) {
            if (distance > OUTSIDE_SOURCE) {
                Robot.isInsideSource = false;
            }
        } else {
            if (distance < INSIDE_SOURCE) {
                Robot.isEnable = false;
                Robot.isInsideSource = true;
            }
        }

    }

    public static void checkEnable() {

    }

    public void checkInitPos() {
        Log.i(thisLogTag,"init test:　isInitPosSucceed: " + Robot.isInitPosSucceed);
        if (Robot.isInitPosSucceed) {

        } else {
            Robot.isEnable = false;
            if (Distance.vector2D(Robot.tagPosition, new double[]{0.0, 0.0}) < 0.5) {
                Log.i(thisLogTag,"init test:　isInitPosSucceed: " + Robot.isInitPosSucceed);
                Robot.isInitPosSucceed = true;
                InitMap.initMap();
                InitNative.initNative();
            }
        }
    }

    public static void checkTrack() {

        if(Robot.track == null)
            return;
        double [] startPoint = new double[2];
        double [] endPoint = new double[2];
        double [] distance = new double[3];
        double distanceMin = 10.0;
        int distanceI = 0;

        for(int i=0; i<Robot.track.length/3; i++) {
            startPoint[0] = Robot.track[i * 3 + 0];
            startPoint[1] = Robot.track[i * 3 + 1];
            if (i == Robot.track.length/3 - 1) {
                endPoint[0] = Robot.track[0];
                endPoint[1] = Robot.track[1];
            } else {
                endPoint[0] = Robot.track[(i + 1) * 3 + 0];
                endPoint[1] = Robot.track[(i + 1) * 3 + 1];
            }
            distance[0] = Distance.vector2D(startPoint,endPoint);
            distance[1] = Distance.vector2D(endPoint,Robot.position);
            distance[2] = Distance.vector2D(Robot.position,startPoint);
            double err = distance[1] + distance[2] - distance[0];
            if (err < distanceMin) {
                distanceMin = err;
                distanceI = i;
            }
        }

        if(Robot.trackWeight != null)
            NativeClass.cSetK(5,Robot.K5 * Robot.trackWeight[distanceI]);

    }


}
